/* 
* @Author: obsidian
* @Date:   2015-12-16 09:43:22
* @Last Modified by:   obsidian
* @Last Modified time: 2016-06-04 09:49:50
*/

#include <stdio.h>
#include "GPSpath.h"
#include "driver/sd/file.h"
#include "module/GPS/GPS.h"
#include "module/pid/pid.h"
#include "driver/math/inc/math.h"
#include "pointpath.h"
#include "module/steer/steer.h"
#include "module/camera/camera.h"
#include "module/gpsteer/gpsteer.h"
#include "module/stopsteer/stopsteer.h"
#include "driver/led/inc/led.h"
#include "control/car.h"
#include "module/lcd/lcd_global.h"
#include "control/store/store.h"
#include "module/brake/brake.h"
#include "module/radar/radar.h"
#include "module/ledboard/ledboard.h"
#include "math.h"
#include "module/def.h"

void gpspath_read_sd(void);
void gps_path_update(void);
void gpspath_init(void);
void gpspath_save(void);
void timer_cfg(void);
float TIM3_IRQHandler(void);
float my_value = 0;

uint32_t myasd = 0;


struct  GPSpath gpspath = 
{
        .status            = 0,                 //status
        .current_index     = 0,                  //current_index
        .direction         = 2,                  //direction
        .target_angle      = 0,                  //target_radian
        .target_point      = {0,0},
        .distance_error    = 0,                  //distance_error
        .angle_error       = 0,                  //angle_error
        .mixed_error       = 0,                  //mixed_error
        .angle_k           = 4.5,                 //angle_k  4.5
        .distance_k        = 1.0,                 //distance_k  1.5
        .camera_k          = 1.8,          //camera_k
        .pre               = 30,                  //pre
        .base_pre          = 15,                  //base_pre
        .camera_center     = 159,                 //camera_center
        .camera_y          = 7250,
        .start_idx         = 0,//40 + 12,//2091, //50
				.timecd						 = 0,
        /*
                function
         */
        .init               = gpspath_init,
        .read_from_sd       = gpspath_read_sd,
        .gps_path_update    = gps_path_update,    
        .timer_driver       = timer_cfg,
        .save               = gpspath_save,
				
};




                    //   0   1    1    2    4    5   6     7    8     9     10   11     12    13  14      15    16   17    18     19   20     21   22    23    24
static int Route[30]={0, 196, 223, 437, 464, 674, 701, 854, 954, 1153, 1248, 1399, 1546, 1661, 1709, 1745, 1803, 2024, 2097, 2467, 2669, 2859, 3060, 3191, 3309, 3790,};
//6-22可用static int Route[30]={0, 196, 223, 437, 464, 674, 701, 878, 978, 1151, 1246, 1411, 1586, 1701, 1749, 1785, 1843, 2064, 2137, 2507, 2709, 2899, 3100, 3231, 3349, 3830, };
//static int Route[30]={0, 196, 223, 437, 464, 674, 701, 878, 978, 1151, 1246, 1400, 1586, 1701, 1749, 1784, 1842, 2061, 2117, 2496, 2698, 2891, 3092, 3223, 3341, 3822,  };
struct Path_point judge_direc(unsigned short index)
{
    struct Path_point   point           = {0};
    if(gps.direction == 2)
    {
        point.point.x = - path[index].point.x;
        point.point.y =   path[index].point.y;      
        point.target_radian = - path[index].target_radian;
    }
    else
    {
        point.point.x =  path[index].point.x;
        point.point.y =  path[index].point.y;
        point.target_radian = path[index].target_radian;        
    }
    return point;
}

void gps_path_update()
{
	float mend = 0;
	float error = 0;
	error = ledboard.getdiserr();
    mend = pid.get_pid(error);
    steer.set_angle(mend);

}

static struct GPSpath dup_gpspath;

void gpspath_read_sd()
{
    unsigned int k = 0;
    car.strjoint("gpspath");
    f_open(&fil,car.path,FA_READ);
    if(fil.fs == 0){return;}
    f_read(&fil, (char *)&dup_gpspath, sizeof(struct GPSpath), &k);
    gpspath.angle_k = dup_gpspath.angle_k;
    gpspath.distance_k = dup_gpspath.distance_k;
    gpspath.camera_k = dup_gpspath.camera_k;
    gpspath.camera_center = dup_gpspath.camera_center;
    gpspath.pre = dup_gpspath.pre;
    gpspath.base_pre = dup_gpspath.base_pre;
    gpspath.start_idx = dup_gpspath.start_idx;
    //gpspath.camera_y = dup_gpspath.camera_y;
    file_close();   
}

void gpspath_init()
{                                                            
    gpspath.current_index   =  gpspath.start_idx;
    //gps.start_point.x       = path[gpspath.current_index].point.x;
    //gps.start_point.y       = path[gpspath.current_index].point.y;
    gps.rear_point.x = gps.start_point.x;
    gps.rear_point.y = gps.start_point.y;
    gps.base_radian = gps.start_angle * PI / 180.0;
    gps.front_point.x = gps.rear_point.x + gps.car_len * sin(gps.base_radian);
    gps.front_point.y = gps.rear_point.y + gps.car_len * cos(gps.base_radian);
}

void gpspath_save()
{
    unsigned int k = 0;
    car.strjoint("gpspath");
    f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);
    if(fil.fs == 0){return;}
    f_write(&fil, (char *)&gpspath, sizeof(struct GPSpath), &k);
    file_close();
}



void timer_cfg(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; 
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; 
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; 
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
    NVIC_Init(&NVIC_InitStructure); 
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5 , ENABLE);
    TIM_TimeBaseStructure.TIM_Prescaler = 72-1; 
    TIM_TimeBaseStructure.TIM_Period = 2500; // 2500 - 2.5ms
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
    TIM_ITConfig(TIM5,TIM_IT_Update,ENABLE);
    TIM_Cmd(TIM5,ENABLE);

}

static unsigned char update_count = 0;
void TIM5_IRQHandler(void)
{   
    
    gps.GPS_update();
    update_count ++;
    if(update_count >= 2)
    {
		
            gpspath.gps_path_update();
            update_count = 0;
    }
   TIM_ClearFlag(TIM5,TIM_IT_Update);
}

void send_str (USART_TypeDef *USARTx, char *str){
    while (*str) USART_SendData(USARTx, *str++);
    USART_SendData(USARTx, '\0');
}

float TIM3_IRQHandler()
{
    TIM_ClearFlag(TIM3, TIM_IT_Update);
	static char encoder_str[16] = {0};
    sprintf(encoder_str, "%.2f", -encoder[1].distance/56.8953f);
    send_str(USART1, encoder_str);
    myasd++;
}

